/*****************************************************************************************/
/*  <listenerVN.cpp is sample listener to use VnLibrary for the other purpose 
    like using Orinetaion, or other values (Quaternion, AngularRate, Acceleration) 
    for EKF position filter
 
    Copyright (C) <iroboApp - 2013>  
    
    <Authors: Anis Koubaa <akoubaa@coins-lab.org> , Yasir Javed <yasir.javed@coins-lab.org> 

    This program is free software: you can redistribute it and/or modify
    it under the terms of the GNU General Public License as published by
    the Free Software Foundation, either version 3 of the License, or
    (at your option) any later version.

    This program is distributed in the hope that it will be useful,
    but WITHOUT ANY WARRANTY; without even the implied warranty of
    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
    GNU General Public License for more details.

    You should have received a copy of the GNU General Public License
    along with this program.  If not, see <http://www.gnu.org/licenses/>.

*/
/*****************************************************************************************/


#include "libvectornav.h" // it contains link to vn100.h and math.h
#include "vn100.h" //need to access vn100 functions
#include "vncp_services.h"
#include "vectornav.h" //define the interface of the class
#include "ros/ros.h"
#include "rosVectorNav/vn100_msg.h" //define the message type of vn100
#include "geometry_msgs/Quaternion.h" //define the message type of VectorSpace(Quaternion To get X ,Y, Z, W)
#include "geometry_msgs/Vector3.h"  //define the message type of 3 Compnenets used to get acceleration magnetic values through VN
#include "sensor_msgs/Imu.h"
// for async threads
#include <boost/thread.hpp>



void vnStatusCallback(rosVectorNav::vn100_msg msg)
{
printf("\n\n--------------vnStatusCallback---------------------\n");
ROS_INFO("I am getting the Status values");
ROS_INFO("I heard Yaw: [%0.3f]",  msg.yaw );
ROS_INFO("I heard Pitch: [%0.3f]",  msg.pitch );
ROS_INFO("I heard Roll: [%0.3f]",  msg.roll );
}

void vnQuaternionCallback(geometry_msgs::Quaternion quanternionMsg)
{
printf("\n\n-------------vnQuaternionCallback----------------------\n");
/*ROS_INFO("I am getting the VectorSpace values");
ROS_INFO("The X is: [%0.3f]",  quanternionMsg.x);
ROS_INFO("The Y is: [%0.3f]",  quanternionMsg.y );
ROS_INFO("THe Z is: [%0.3f]",  quanternionMsg.z );
ROS_INFO("THe W is: [%0.3f]",  quanternionMsg.w );

  */
}

void vnAccelerationCallback(geometry_msgs::Vector3 msg)
{
printf("\n\n-----------vnAccelerationCallback------------------------\n");
/*ROS_INFO("I am getting the Acceleration values");
ROS_INFO("Acceleration on X: [%0.3f]",  msg.x );
ROS_INFO("Acceleration on Y: [%0.3f]",  msg.y );
ROS_INFO("Acceleration on Z: [%0.3f]",  msg.z );
*/
}

void vnAngularRateCallback(geometry_msgs::Vector3 angulRateMsg)
{
printf("\n\n----------------AngularRateCallback-------------------\n");
/*ROS_INFO("I am getting the Angular Rate values");
ROS_INFO("AngularRate on X: [%0.3f]",  angulRateMsg.x );
ROS_INFO("AngularRate on Y: [%0.3f]",  angulRateMsg.y );
ROS_INFO("AngularRate on Z: [%0.3f]",  angulRateMsg.z );
*/
  
}

void vnIMUCallback(sensor_msgs::Imu msgImu)
{
printf("\n\n--------------vnIMUCallback---------------------\n");
ROS_INFO("I am getting the Data values");
ROS_INFO("Linear Acceleration on X: [%0.3f]",  msgImu.linear_acceleration.x);
ROS_INFO("Linear Acceleration on Y: [%0.3f]", msgImu.linear_acceleration.y);
ROS_INFO("Linear Acceleration on Z: [%0.3f]",  msgImu.linear_acceleration.z );

ROS_INFO("Angular Velocity on X: [%0.3f]",    msgImu.angular_velocity.x);
ROS_INFO("Angular Velocity on Y: [%0.3f]", msgImu.angular_velocity.y);
ROS_INFO("Angular Velocity on Z: [%0.3f]",   msgImu.angular_velocity.z );  

ROS_INFO("Orientation X: [%0.3f]", msgImu.orientation.x);
ROS_INFO("Orientation y: [%0.3f]",   msgImu.orientation.y );  

ROS_INFO("Orientation z: [%0.3f]",    msgImu.orientation.z);
ROS_INFO("Orientation w: [%0.3f]", msgImu.orientation.w);

}


int main(int argc, char **argv)
{
  ros::init(argc, argv, "listenerVN");
  ros::NodeHandle n;
  ROS_INFO("Vector NAV Listener subscriber");
  //ros::Subscriber sub = n.subscribe("odom", 10, odomCallback);
 ros::Subscriber subVN = n.subscribe("vn_ypr_topic", 1, vnStatusCallback);
 ros::Subscriber subQuan = n.subscribe("vn_quaternion_topic", 1, vnQuaternionCallback);
 ros::Subscriber subAccel = n.subscribe("vn_acceleration_topic", 1, vnAccelerationCallback);
 ros::Subscriber subAngRate = n.subscribe("vn_angular_rate_topic", 1, vnAngularRateCallback);
 ros::Subscriber subImu = n.subscribe("imu_data", 1, vnIMUCallback);
      ros::AsyncSpinner vnStatusSpinner(5);
      vnStatusSpinner.start();
      ros::Rate threadRate(12);
   //ros::spin();
   //int i=0;
     while (ros::ok()){ // rosok bracket
     threadRate.sleep();
 // ros::spin();
      }
      
  return 0;
}

